﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.Windows.Media.Media3D;
using System.Windows.Controls.Primitives;
using System.Configuration;
using System.Timers;
using System.Windows.Media.Animation;
using System.IO;

using Wilke.Interactive.Drone.Control;
using Wilke.Interactive.Drone.Control.Enumerations;
using Wilke.Interactive.Drone.Contracts;
using Wilke.Interactive.Drone.InputProviders;
using Wilke.Interactive.Drone.Extensions;
using System.Diagnostics;

namespace Wilke.Interactive.Drone.PlayGround
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        #region Declarations

        private DroneController droneController = null;
        private DroneControllerConfiguration droneControllerConfiguration = null;
        private Rectangle visionDetectBox;
        private Random random = null;
        private IInputProvider inputProvider;

        private float pitch = 0;
        private float roll = 0;
        private float height = 0;
        private float yaw = 0;

        private Timer ScreenInputTimer { get; set; }

        private bool simulate = false;

        private string picturesPath = @"F:\Users\Wilke\Desktop\DroneFiles\Pictures";
        private string videosPath = @"F:\Users\Wilke\Desktop\DroneFiles\Videos";

        #endregion

        #region Private Properties

        public SolidColorBrush StatusGreen
        {
            get { return this.Resources["StatusGreen"] as SolidColorBrush; }
        }

        #endregion

        #region Construction

        public MainWindow()
        {
            InitializeComponent();
            ShowInTaskbar = false;
        }

        #endregion

        #region DroneController Operations

        private void Connect()
        {
            #region Initialize DroneController

            droneControllerConfiguration = new DroneControllerConfiguration();

            droneControllerConfiguration.NetworkIdentifier = "ardrone_229";
            droneControllerConfiguration.EnableNavigationDataThread = true;
            droneControllerConfiguration.EnableVideoStreamThread = true;
            droneControllerConfiguration.EnableATCommandThread = true;
            droneControllerConfiguration.EnableControlInfoThread = false;
            droneControllerConfiguration.VideoFilePath = videosPath;
            droneControllerConfiguration.PictureFilePath = picturesPath;
            droneControllerConfiguration.EnableATCommandSimulation = simulate;
            droneControllerConfiguration.EnableInputFeedback = true;

            #region Throttle Values

            droneControllerConfiguration.RollThrottle = .2f;
            droneControllerConfiguration.PitchThrottle = .2f;
            droneControllerConfiguration.YawThrottle = .6f;
            droneControllerConfiguration.HeightThrottle = .4f;

            #endregion

            droneController = new DroneController(droneControllerConfiguration);

            droneController.TraceNotificationLevel = TraceNotificationLevel.Verbose;
            droneController.OnNotifyTraceMessage += new EventHandler<TraceNotificationEventArgs>(droneController_NotifyTraceMessage);
            droneController.OnNotifyVideoMessage += new EventHandler<VideoNotificationEventArgs>(droneController_NotifyVideoMessage);
            droneController.OnNotifyDroneInfoMessage += new EventHandler<DroneInfoNotificationEventArgs>(droneController_NotifyDroneInfoMessage);
            droneController.OnConnectionStatusChanged += new EventHandler<ConnectionStatusChangedEventArgs>(droneController_OnConnectionStatusChanged);
            droneController.OnNotifyInputMessage += new EventHandler<InputNotificationEventArgs>(droneController_OnNotifyInputMessage);
            droneController.OnVisionDetectMessage += new EventHandler<VisionDetectNotificationEventArgs>(droneController_OnVisionDetectMessage);

            #endregion

            if (droneControllerConfiguration.EnableATCommandSimulation)
            {
                MessageBox.Show("Connected in simulation mode.");
            }

            droneController.Connect();
        }

        int noDetectionCount = 0;
        float detectionYaw = 0;
        float detectionHeight = 0;
        float detectionPitch = 0;

        void droneController_OnVisionDetectMessage(object sender, VisionDetectNotificationEventArgs e)
        {
            if (!Dispatcher.CheckAccess())
            {
                Dispatcher.Invoke(new Action<object, VisionDetectNotificationEventArgs>(droneController_OnVisionDetectMessage), sender, e);
            }
            else
            {
                if (drawingSurface.Children.Count == 0)
                {
                    visionDetectBox = new Rectangle();
                    visionDetectBox.Stroke = Brushes.Yellow;
                    visionDetectBox.StrokeThickness = 4;
                    drawingSurface.Children.Add(visionDetectBox);
                }

                if (e.IsDetected)
                {
                    int boxHeight = e.BoxHeight + 50;
                    visionDetectBox.Margin = new Thickness(((e.BoxLeft - e.BoxWidth / 2 - 10) * imageDroneCamera.ActualWidth) / Constants.DetectionViewPortWidth, ((e.BoxTop - boxHeight / 2) * imageDroneCamera.ActualHeight) / Constants.DetectionViewPortHeight, 0, 0);
                    visionDetectBox.Width = ((e.BoxWidth + 20) * imageDroneCamera.ActualWidth) / Constants.DetectionViewPortWidth;
                    visionDetectBox.Height = ((boxHeight) * imageDroneCamera.ActualHeight) / Constants.DetectionViewPortHeight;
                    visionDetectBox.Visibility = System.Windows.Visibility.Visible;

                    if (e.TagDistance > 100)
                    {
                        detectionPitch = -0.2f;
                    }
                    if (e.TagDistance < 50)
                    {
                        detectionPitch = 0.2f;
                    }

                    int boxCenterX = e.BoxLeft + (e.BoxWidth / 2);
                    int boxCenterY = e.BoxTop + (e.BoxHeight / 2);

                    if (boxCenterX > 500)
                    {
                        detectionYaw = 0.9f;
                    }
                    else if (boxCenterX < 500)
                    {
                        detectionYaw = -0.9f;
                    }

                    if (boxCenterY > 500)
                    {
                        detectionHeight = -0.5f;
                    }
                    else if (boxCenterY < 500)
                    {
                        detectionHeight = 0.5f;
                    }

                    droneController.SetFlightParameters(0, detectionPitch, detectionHeight, detectionYaw);
                    droneController.PlayLedAnimation(LedAnimation.BlinkGreen, 10, 1);
                }
                else
                {
                    visionDetectBox.Visibility = System.Windows.Visibility.Hidden;
                    droneController.PlayLedAnimation(LedAnimation.BlinkRed, 10, 1);

                    noDetectionCount++;

                    if (noDetectionCount > 20)
                    {
                        detectionPitch = 0;
                        detectionHeight = 0;
                        detectionYaw = 0;
                        noDetectionCount = 0;
                        Debug.Print("Resetting detectionValues");
                    }
                }
            }
        }

        void droneController_OnNotifyInputMessage(object sender, InputNotificationEventArgs e)
        {
            if (!Dispatcher.CheckAccess())
            {
                Dispatcher.Invoke(new Action<object, InputNotificationEventArgs>(droneController_OnNotifyInputMessage), sender, e);
            }
            else
            {
                //We do not want to interact with screen controls if inputprovider is screen itself.
                if (comboboxInputProvider.SelectedIndex > 0)
                {
                    radarScreen.Roll = e.Roll;
                    radarScreen.Pitch = e.Pitch;

                    if (tabControlSwitch.SelectedIndex == 0)
                    {
                        joystickBase.DroneHeight = e.Height;
                        joystickBase.Yaw = e.Yaw;
                    }
                    else
                    {
                        sliderHeight.SliderValue = e.Height;
                        sliderYaw.SliderValue = e.Yaw;
                    }
                }
            }
        }

        void droneController_OnConnectionStatusChanged(object sender, ConnectionStatusChangedEventArgs e)
        {
            if (!Dispatcher.CheckAccess())
            {
                Dispatcher.Invoke(new Action<object, ConnectionStatusChangedEventArgs>(droneController_OnConnectionStatusChanged), sender, e);
            }
            else
            {
                if (e.ConnectionStatus == ConnectionStatus.Open)
                {
                    buttonHeader.SetConnectState(true);

                    if (comboboxInputProvider.SelectedIndex == 0)
                    {
                        PrepareScreenInputProvider(true);
                    }
                }
                else
                {
                    buttonHeader.SetConnectState(false);
                }
            }
        }

        private void Disconnect()
        {
            if (droneController != null)
            {
                if (droneController.DroneIsFlying)
                {
                    ClearInputProvider();

                    droneController.StopEngines();
                }

                if (droneController.DroneIsRecording)
                {
                    droneController.StopRecordVideo();
                }

                droneController.Disconnect();

                listBox1.Items.Clear();
                buttonDroneNavigationStatus.InnerButtonColor = Brushes.Red;
                buttonTakeOff.Content = "Take Off";
            }

            if (comboboxInputProvider.SelectedIndex == 0)
            {
                PrepareScreenInputProvider(false);
            }

            comboboxInputProvider.SelectedIndex = 0;
            imageDroneCamera.Source = new BitmapImage(new Uri("/Resources/Images/Default.png", UriKind.Relative));
            imageBatteryLevel.Source = new BitmapImage(new Uri("/Resources/Images/BatteryDisabled.png", UriKind.Relative));
            buttonDroneNavigationStatus.InnerButtonColor = Brushes.SlateGray;
            buttonRecordingStatus.InnerButtonColor = Brushes.SlateGray;
            buttonHeader.SetConnectState(false);
            if (visionDetectBox != null)
            {
                visionDetectBox.Visibility = System.Windows.Visibility.Hidden;
            }
        }

        private void PrepareScreenInputProvider(bool enable)
        {
            radarScreen.DroneCoordinatesChanged -= new EventHandler<DroneCoordinatesChangedEventArgs>(DroneCoordinatesChangedEventHandler);
            joystickBase.DroneCoordinatesChanged -= new EventHandler<DroneCoordinatesChangedEventArgs>(DroneCoordinatesChangedEventHandler);
            sliderHeight.SliderValueChanged -= new EventHandler<SliderValueChangedEventArgs>(sliderHeight_SliderValueChanged);
            sliderYaw.SliderValueChanged -= new EventHandler<SliderValueChangedEventArgs>(sliderYaw_SliderValueChanged);

            if (enable)
            {
                radarScreen.DroneCoordinatesChanged += new EventHandler<DroneCoordinatesChangedEventArgs>(DroneCoordinatesChangedEventHandler);
                joystickBase.DroneCoordinatesChanged += new EventHandler<DroneCoordinatesChangedEventArgs>(DroneCoordinatesChangedEventHandler);
                sliderHeight.SliderValueChanged += new EventHandler<SliderValueChangedEventArgs>(sliderHeight_SliderValueChanged);
                sliderYaw.SliderValueChanged += new EventHandler<SliderValueChangedEventArgs>(sliderYaw_SliderValueChanged);
            }
        }

        private void ClearInputProvider()
        {
            if (inputProvider != null)
            {
                inputProvider.Dispose();
            }
        }

        void droneController_NotifyDroneInfoMessage(object sender, DroneInfoNotificationEventArgs e)
        {
            this.Dispatcher.Invoke(new Action<DroneInfoNotificationEventArgs>(x =>
            {
                buttonDroneNavigationStatus.InnerButtonColor = (e.WorkerThreadNavigationDataActive) ? StatusGreen : Brushes.Red;

                if (droneController.DroneIsFlying)
                {
                    if (buttonTakeOff.Content.ToString() != "Land")
                    {
                        buttonTakeOff.Content = "Land";
                    }
                }

                int batteryLevel = e.BatteryLevel / 25;

                batteryLevel = (batteryLevel > 3) ? 3 : batteryLevel;

                string batteryImage = String.Format("Battery{0}.png", batteryLevel);

                if (!imageBatteryLevel.Source.ToString().Contains(batteryImage))
                {
                    imageBatteryLevel.Source = new BitmapImage(new Uri("/Resources/Images/" + batteryImage, UriKind.Relative));
                }

                imageBatteryLevel.ToolTip = String.Format("{0} %", e.BatteryLevel);

                if (droneController.DroneIsRecording)
                {
                    if (!buttonRecordingStatus.IsBlinking)
                    {
                        buttonRecordingStatus.InnerButtonColor = Brushes.Red;
                        buttonRecordingStatus.IsBlinking = true;
                    }
                }
                else
                {
                    if (buttonRecordingStatus.InnerButtonColor != Brushes.DimGray)
                    {
                        buttonRecordingStatus.InnerButtonColor = Brushes.DimGray;
                    }
                }

            }), e);
        }

        void droneController_NotifyTraceMessage(object sender, TraceNotificationEventArgs e)
        {
            #region Process Message Notification

            string message = String.Format("{0} - {1}", e.NotificationLevel.ToString(), e.NotificationMessage);
            if (Dispatcher.CheckAccess())
            {
                if (listBox1.Items.Count > 512)
                {
                    listBox1.Items.Clear();
                    listBox1.Items.Add("Trace cleared.");

                }
                listBox1.Items.Add(message);
                listBox1.ScrollIntoView(listBox1.Items[listBox1.Items.Count - 1]);
            }
            else
            {
                Dispatcher.Invoke(new Action<String>(x =>
                {
                    if (listBox1.Items.Count > 512)
                    {
                        listBox1.Items.Clear();
                        listBox1.Items.Add("Trace cleared.");

                    }
                    listBox1.Items.Add(x);
                    listBox1.ScrollIntoView(listBox1.Items[listBox1.Items.Count - 1]);
                }), message);
            }


            #endregion
        }

        void droneController_NotifyVideoMessage(object sender, VideoNotificationEventArgs e)
        {
            #region Process Video Notification

            this.Dispatcher.Invoke(new Action<ImageSource>(x =>
            {
                imageDroneCamera.Source = x;
                imageDroneCamera.InvalidateVisual();
            }), e.CurrentImage);

            #endregion
        }

        private void DroneCoordinatesChangedEventHandler(object sender, DroneCoordinatesChangedEventArgs e)
        {
            roll = e.Roll * droneControllerConfiguration.RollThrottle;
            pitch = e.Pitch * droneControllerConfiguration.PitchThrottle;
            yaw = e.Yaw * droneControllerConfiguration.YawThrottle; ;
            height = e.Height * droneControllerConfiguration.HeightThrottle;
        }

        #endregion

        #region Event Handlers

        private void Window_PreviewKeyDown(object sender, KeyEventArgs e)
        {
            switch (e.Key)
            {
                case Key.Escape:
                    if (this.WindowState == WindowState.Maximized)
                    {
                        this.WindowState = WindowState.Normal;
                        this.WindowStyle = WindowStyle.ThreeDBorderWindow;
                    }
                    break;
            }
        }

        private void Window_SizeChanged(object sender, SizeChangedEventArgs e)
        {
            buttonHeader.SelectSegment(1, this.WindowState == WindowState.Maximized);
            viewboxRadar.Height = viewboxRadar.Width;
        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            DroneController droneController = new DroneController();

            buttonHeader.Title = (simulate) ? "Drone Interactive Demo (simulation mode)" : "Drone Interactive Demo";

            random = new Random();
        }

        private void buttonHeader_SegmentClicked(object sender, SegmentClickedEventArgs e)
        {
            switch (e.SegmentIndex)
            {
                case 0:

                    if (droneController == null)
                    {
                        Connect();
                    }
                    else
                    {
                        switch (droneController.ConnectionStatus)
                        {
                            case ConnectionStatus.Closed:
                            case ConnectionStatus.Error:
                                Connect();
                                break;
                            case ConnectionStatus.Open:
                                Disconnect();
                                break;
                            default:
                                Disconnect();
                                break;
                        }
                    }
                    break;
                case 2:
                    Application.Current.Shutdown();
                    break;
            }
        }

        private void buttonTakeOff_Click(object sender, RoutedEventArgs e)
        {
            //MessageBox.Show("Temporarely deactivated");
            buttonTakeOff.Content = (buttonTakeOff.Content.ToString().Contains("Take")) ? "Land" : "Take Off";

            if (buttonTakeOff.Content.ToString().Contains("Take"))
            {
                droneController.StopEngines();
                StopScreenInputTimer();
            }
            else
            {
                droneController.SetFlatTrim();
                droneController.StartEngines();
                if (comboboxInputProvider.SelectedIndex == 0)
                {
                    StartScreenInputTimer();
                }
            }
        }

        private void buttonSwitch_Click(object sender, RoutedEventArgs e)
        {
            if (((Button)sender).Name.EndsWith("1"))
            {
                tabControlSwitch.SelectedIndex = 1;
            }
            else
            {
                tabControlSwitch.SelectedIndex = 0;
            }
        }

        private void mainWindow_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            if (droneController != null)
            {
                StopScreenInputTimer();
                droneController.StopEngines();
                droneController.StartReset();
                droneController.Disconnect();
            }
        }

        private void StartScreenInputTimer()
        {
            ScreenInputTimer = new Timer(100);
            ScreenInputTimer.Elapsed += new ElapsedEventHandler(ScreenInputTimer_Elapsed);
            ScreenInputTimer.Start();
        }

        private void StopScreenInputTimer()
        {
            if (ScreenInputTimer != null)
            {
                ScreenInputTimer.Stop();
            }
        }

        private void ScreenInputTimer_Elapsed(object sender, ElapsedEventArgs e)
        {
            if (droneController != null)
            {
                droneController.SetFlightParameters(roll, pitch, height, yaw);
            }
        }

        private void ResetDrone(bool shouldStartReset)
        {
            if (droneController != null)
            {
                if (shouldStartReset)
                {
                    droneController.StartReset();
                }
                else
                {
                    droneController.StopReset();
                }
            }
        }

        private void buttonResetDrone_MouseDown(object sender, MouseButtonEventArgs e)
        {
            ResetDrone(true);
        }

        private void buttonResetDrone_MouseUp(object sender, MouseButtonEventArgs e)
        {
            ResetDrone(false);
        }

        private void buttonResetDrone_TouchDown(object sender, TouchEventArgs e)
        {
            ResetDrone(true);
        }

        private void buttonResetDrone_TouchUp(object sender, TouchEventArgs e)
        {
            ResetDrone(false);
        }

        private void buttonResetDrone_StylusDown(object sender, StylusDownEventArgs e)
        {
            ResetDrone(true);
        }

        private void buttonResetDrone_StylusUp(object sender, StylusEventArgs e)
        {
            ResetDrone(false);
        }

        private void buttonResetDrone_PreviewMouseDown(object sender, MouseButtonEventArgs e)
        {
            ResetDrone(true);
        }

        private void buttonResetDrone_PreviewMouseUp(object sender, MouseButtonEventArgs e)
        {
            ResetDrone(false);
        }

        private void buttonResetDrone_PreviewStylusDown(object sender, StylusDownEventArgs e)
        {
            ResetDrone(true);
        }

        private void buttonResetDrone_PreviewStylusUp(object sender, StylusEventArgs e)
        {
            ResetDrone(false);
        }

        private void buttonResetDrone_PreviewTouchDown(object sender, TouchEventArgs e)
        {
            ResetDrone(true);
        }

        private void buttonResetDrone_PreviewTouchUp(object sender, TouchEventArgs e)
        {
            ResetDrone(false);
        }

        private void comboboxInputProvider_SelectionChanged(object sender, SelectionChangedEventArgs e)
        {
            if (droneController == null)
            {
                if (comboboxInputProvider.SelectedIndex != 0)
                {
                    MessageBox.Show("Connect before selecting an input provider.");
                    comboboxInputProvider.SelectedIndex = 0;
                }
                return;
            }

            SelectInputProvider(comboboxInputProvider.SelectedIndex);            
        }

        void sliderYaw_SliderValueChanged(object sender, SliderValueChangedEventArgs e)
        {
            this.yaw = e.SliderValue;
        }

        void sliderHeight_SliderValueChanged(object sender, SliderValueChangedEventArgs e)
        {
            this.height = e.SliderValue;
        }

        private void buttonRecordVideo_Click(object sender, RoutedEventArgs e)
        {
            if (buttonRecordVideo.IsChecked.Value)
            {
                StartRecordVideo();
            }
            else
            {
                StopRecordVideo();
            }
        }

        private void StartRecordVideo()
        {
            if (droneController != null)
            {
                droneController.StartRecordVideo(videosPath);
                buttonRecordingStatus.IsBlinking = true;
            }
        }

        private void StopRecordVideo()
        {
            if (droneController != null)
            {
                droneController.StopRecordVideo();
                buttonRecordingStatus.IsBlinking = false;
                buttonRecordingStatus.InnerButtonColor = Brushes.DimGray;
            }
        }

        private void buttonTakePicture_Click(object sender, RoutedEventArgs e)
        {
            if (droneController != null)
            {
                droneController.TakePicture(picturesPath);
            }
        }

        private void buttonPlayLed_Click(object sender, RoutedEventArgs e)
        {
            if (droneController != null)
            {
                int animationId = random.Next(1, 6);
                droneController.PlayLedAnimation(animationId, 10, 3);
            }
        }

        private void buttonSwitchVideoChannel_Click(object sender, RoutedEventArgs e)
        {
            if (droneController != null)
            {
                droneController.SwitchVideoChannel(VideoChannel.Next);
            }
        }

        private void buttonDesignPath_Click(object sender, RoutedEventArgs e)
        {
            if (droneController == null)
            {
                MessageBox.Show("Connect before using this feature.");
                return;
            }

            inputProvider = new AssistInputProvider(droneController);
            inputProvider.Initialize(droneControllerConfiguration.RollThrottle, droneControllerConfiguration.PitchThrottle, droneControllerConfiguration.HeightThrottle, droneControllerConfiguration.YawThrottle);

            PathDesigner pathDesigner = new PathDesigner(droneController);
            pathDesigner.ShowDialog();

            //Switch back to the selected inputprovider
            SelectInputProvider(comboboxInputProvider.SelectedIndex);  
        }
      
        private void SelectInputProvider(int inputProviderIndex)
        { 
            try
            {
                switch (inputProviderIndex)
                {
                    case 0:
                        ClearInputProvider();
                        break;
                    case 1:
                        inputProvider = new JoystickInputProvider(droneController);
                        break;
                    case 2:
                        inputProvider = new WiimoteInputProvider(droneController);
                        break;
                    case 3:
                        inputProvider = new FlexisInputProvider(droneController);
                        break;
                    default:
                        break;
                }

                if (inputProvider != null)
                {
                    inputProvider.Initialize(droneControllerConfiguration.RollThrottle, droneControllerConfiguration.PitchThrottle, droneControllerConfiguration.HeightThrottle, droneControllerConfiguration.YawThrottle);
                }
            }
            catch (Exception exception)
            {
                MessageBox.Show(exception.Message);
            }
            finally
            {
                StopScreenInputTimer();
            }
        }

        private void buttonFollowMe_Click(object sender, RoutedEventArgs e)
        {
            if (buttonFollowMe.IsChecked.Value)
            {
                if (droneController == null)
                {
                    MessageBox.Show("Connect before using this feature.");
                    return;
                }

                inputProvider = new AssistInputProvider(droneController);
                inputProvider.Initialize(droneControllerConfiguration.RollThrottle, droneControllerConfiguration.PitchThrottle, droneControllerConfiguration.HeightThrottle, droneControllerConfiguration.YawThrottle);

                droneController.SetIndoorConfiguration();
                droneController.StartVisionDetect();
            }
            else
            {
                droneController.StopVisionDetect();
                //Switch back to the selected inputprovider
                SelectInputProvider(comboboxInputProvider.SelectedIndex);  
            }
        }

        #endregion
    }
}
